#!/usr/bin/env python3
# log_replay_ui.py
# 日志回放工具：用于可视化日志数据回放
import sys
import time
import numpy as np
import math
import os
import re
import datetime
from PyQt5.QtWidgets import (
    QApplication,
    QMainWindow,
    QWidget,
    QVBoxLayout,
    QHBoxLayout,
    QLabel,
    QSplitter,
    QPushButton,
    QStatusBar,
    QSlider,
)
from PyQt5.QtGui import QPixmap, QImage, QPainter, QPen, QColor, QBrush, QFont
from PyQt5.QtCore import Qt, QTimer, QRect
import cv2
import yaml


# Import the LogbagParser class
class LogbagParser:
    """
    A class to load and parse the logbag files for visualization replay.
    Log format example:
    [2025-10-19 14:30:45.123] evader:(1.23,4.56) p1:(-1.00,-1.00) p2:(7.89,0.12) [n0:0.500 n3:0.250 n5:0.250]

    - Timestamp: YYYY-MM-DD HH:MM:SS.mmm
    - Positions: evader:(x,y) p1:(x,y) p2:(x,y)  (default -1,-1 if missing)
    - Beliefs: [n<node>:<prob> ...] only if non-empty, nodes in order of points.
    """

    def __init__(self, filename: str):
        """
        Initialize the parser with the logbag file.

        Args:
            filename (str): Path to the logbag file.
        """
        self.filename = filename
        self.entries: list = []
        self._load_and_parse()

    def _load_and_parse(self):
        """Load the file and parse each line into structured data."""
        with open(self.filename, "r") as f:
            for line_num, line in enumerate(f, 1):
                line = line.strip()
                if not line:
                    continue

                # Parse timestamp
                timestamp_match = re.match(
                    r"\[(\d{4}-\d{2}-\d{2} \d{2}:\d{2}:\d{2}\.\d{3})\]", line
                )
                if not timestamp_match:
                    print(f"Warning: Invalid timestamp in line {line_num}: {line}")
                    continue
                timestamp_str = timestamp_match.group(1)
                try:
                    timestamp = datetime.datetime.strptime(
                        timestamp_str, "%Y-%m-%d %H:%M:%S.%f"
                    )
                except ValueError:
                    print(
                        f"Warning: Invalid timestamp format in line {line_num}: {timestamp_str}"
                    )
                    continue

                # Extract positions
                pos_pattern = r"(\w+):(\([-\d.]+,[-\d.]+\))"
                pos_matches = re.findall(pos_pattern, line)
                positions = {
                    "evader": (-1.0, -1.0),
                    "p1": (-1.0, -1.0),
                    "p2": (-1.0, -1.0),
                }
                for role, pos_str in pos_matches:
                    if role == "evader":
                        key = "evader"
                    elif role.startswith("p"):
                        key = role
                    else:
                        continue
                    # Parse (x,y)
                    xy_match = re.match(r"\(([-\d.]+),([-\d.]+)\)", pos_str)
                    if xy_match:
                        x, y = float(xy_match.group(1)), float(xy_match.group(2))
                        positions[key] = (x, y)

                # Extract beliefs if present
                belief_match = re.search(r"\[(n\d+:[.\d]+(?:\s+n\d+:[.\d]+)*)\]", line)
                beliefs = {}
                if belief_match:
                    belief_items = belief_match.group(1).split()
                    for item in belief_items:
                        node_prob = item.split(":")
                        if len(node_prob) == 2:
                            node = int(node_prob[0][1:])  # Remove 'n' prefix
                            prob = float(node_prob[1])
                            if prob > 0:
                                beliefs[node] = prob

                entry = {
                    "timestamp": timestamp,
                    "positions": positions,
                    "beliefs": beliefs,
                }
                self.entries.append(entry)

        print(f"Loaded {len(self.entries)} entries from {self.filename}")

    def get_entry(self, index: int):
        """
        Get a specific entry by index.

        Args:
            index (int): Index of the entry (0-based).

        Returns:
            Dict: The entry dict, or None if out of range.
        """
        if 0 <= index < len(self.entries):
            return self.entries[index]
        return None

    def get_entry_by_time(self, target_time):
        """
        Find the entry closest to the target time.

        Args:
            target_time (datetime.datetime): The target timestamp.

        Returns:
            Dict: The closest entry dict.
        """
        if not self.entries:
            return None

        # Binary search for closest timestamp
        left, right = 0, len(self.entries) - 1
        while left <= right:
            mid = (left + right) // 2
            if self.entries[mid]["timestamp"] < target_time:
                left = mid + 1
            else:
                right = mid - 1

        # Check closest between left-1 and left
        if left == 0:
            return self.entries[0]
        elif left == len(self.entries):
            return self.entries[-1]

        prev = self.entries[left - 1]
        next_entry = self.entries[left]
        if (target_time - prev["timestamp"]).total_seconds() < (
            next_entry["timestamp"] - target_time
        ).total_seconds():
            return prev
        return next_entry

    def get_timestamps(self):
        """
        Get list of all timestamps.

        Returns:
            List[datetime.datetime]: Sorted list of timestamps.
        """
        return [entry["timestamp"] for entry in self.entries]


# Import PursuitPolicy (assume it's available)
from pur_policy_localD import PursuitPolicy


class GraphMapWidget(QWidget):
    def __init__(self, graph_manager=None, parent=None):
        super().__init__(parent)
        self.graph = graph_manager
        self.robots = []
        self.belief_pr = None
        self.belief_pos = None
        self.scale_factor = 1.0
        self.offset_x = 0
        self.offset_y = 0
        self.capture_count = 0
        # 地图相关
        self.map_pixmap = None
        self.map_width = 0
        self.map_height = 0
        self.resolution = 0.05  # 默认分辨率
        self.origin = [0.0, 0.0]
        self.map_offset_x = 0
        self.map_offset_y = 0
        self.map_scale_factor = 1.0
        # 定义角色颜色
        self.role_colors = {
            "evader": QColor(255, 0, 0),  # 红色 - 逃避者
            "pursuer": QColor(0, 0, 255),  # 蓝色 - 追捕者
        }
        self.setMinimumSize(800, 600)
        self.setMouseTracking(True)
        # 加载地图
        self.load_map()
        if self.map_pixmap is None:
            if self.graph is not None:
                self.update_graph_view()
        else:
            self.update_map_view()

    def load_map(self):
        """加载地图yaml和pgm文件"""
        yaml_path = os.path.abspath(
            os.path.join(os.path.dirname(__file__), "../datas/outdoor_final.yaml")
        )
        if os.path.exists(yaml_path):
            try:
                with open(yaml_path, "r") as f:
                    map_data = yaml.safe_load(f)
                image_rel = map_data["image"]
                image_path = os.path.abspath(
                    os.path.join(os.path.dirname(yaml_path), image_rel)
                )
                self.map_image = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
                if self.map_image is not None:
                    self.map_height, self.map_width = self.map_image.shape[:2]
                    self.resolution = map_data["resolution"]
                    self.origin = map_data["origin"][:2]  # 只取x,y，忽略z
                    h, w = self.map_height, self.map_height
                    qimg = QImage(
                        self.map_image.data, w, h, w, QImage.Format_Grayscale8
                    )
                    self.map_pixmap = QPixmap.fromImage(qimg)
                    print(
                        f"地图加载成功: {image_path}, 分辨率: {self.resolution}, 原点: {self.origin}"
                    )
                else:
                    print(f"无法读取地图图像: {image_path}")
            except Exception as e:
                print(f"加载地图失败: {e}")
        else:
            print(f"地图YAML文件不存在: {yaml_path}")

    def update_map_view(self):
        """更新地图视图变换"""
        if self.map_pixmap is None:
            return
        map_w = self.map_pixmap.width()
        map_h = self.map_pixmap.height()
        scale_x = self.width() / map_w
        scale_y = self.height() / map_h
        self.map_scale_factor = min(scale_x, scale_y)
        scaled_w = map_w * self.map_scale_factor
        scaled_h = map_h * self.map_scale_factor
        self.map_offset_x = (self.width() - scaled_w) / 2
        self.map_offset_y = (self.height() - scaled_h) / 2

    def update_graph_view(self):
        """更新图视图变换（无地图时）"""
        if (
            not hasattr(self.graph, "node_coordinates")
            or not self.graph.node_coordinates
        ):
            return
        # 找到节点的边界
        min_x = min(x for x, y in self.graph.node_coordinates)
        max_x = max(x for x, y in self.graph.node_coordinates)
        min_y = min(y for x, y in self.graph.node_coordinates)
        max_y = max(y for x, y in self.graph.node_coordinates)
        # 计算缩放因子
        width_ratio = (self.width() - 100) / (max_x - min_x) if max_x - min_x > 0 else 1
        height_ratio = (
            (self.height() - 100) / (max_y - min_y) if max_y - min_y > 0 else 1
        )
        self.scale_factor = min(width_ratio, height_ratio)
        # 计算偏移量以使图形居中
        self.offset_x = (
            self.width() - (max_x - min_x) * self.scale_factor
        ) / 2 - min_x * self.scale_factor
        self.offset_y = (
            self.height() - (max_y - min_y) * self.scale_factor
        ) / 2 - min_y * self.scale_factor

    def world_to_pixel(self, x, y):
        """将世界坐标转换为像素坐标"""
        if self.map_pixmap is not None:
            # 世界坐标到地图像素（考虑origin和resolution）
            map_px = (x - self.origin[0]) / self.resolution
            map_py = self.map_height - (
                (y - self.origin[1]) / self.resolution
            )  # Y轴翻转
            # 地图像素到widget像素（使用地图缩放和偏移）
            wx = self.map_offset_x + map_px * self.map_scale_factor
            wy = self.map_offset_y + map_py * self.map_scale_factor
            return int(wx), int(wy)
        else:
            # 无地图时的旧逻辑（基于节点坐标的缩放）
            if (
                not hasattr(self.graph, "node_coordinates")
                or not self.graph.node_coordinates
            ):
                return int(x), int(y)
            # 计算缩放和平移后的位置
            scaled_x = x * self.scale_factor + self.offset_x
            scaled_y = y * self.scale_factor + self.offset_y
            # 反转Y轴：将数学坐标系转换为屏幕坐标系
            flipped_y = self.height() - scaled_y
            return int(scaled_x), int(flipped_y)

    def paintEvent(self, event):
        """绘制地图和机器人"""
        painter = QPainter(self)
        painter.setRenderHint(QPainter.Antialiasing)
        # 绘制底图
        if self.map_pixmap is not None:
            self.update_map_view()  # 更新变换
            map_w = self.map_pixmap.width()
            map_h = self.map_pixmap.height()
            scaled_w = map_w * self.map_scale_factor
            scaled_h = map_h * self.map_scale_factor
            # 修正：使用 QRect 目标矩形，实现整个 pixmap 的自动缩放到居中位置
            target_rect = QRect(
                int(self.map_offset_x),
                int(self.map_offset_y),
                int(scaled_w),
                int(scaled_h),
            )
            painter.drawPixmap(
                target_rect, self.map_pixmap
            )  # 自动缩放整个源到 target_rect
        else:
            # 无地图时填充背景
            painter.fillRect(self.rect(), QColor(240, 240, 240))
            if (
                not hasattr(self.graph, "node_coordinates")
                or not self.graph.node_coordinates
            ):
                painter.setPen(QPen(Qt.black))
                painter.setFont(QFont("Arial", 12))
                painter.drawText(self.rect(), Qt.AlignCenter, "没有地图数据可用")
                return
        # 绘制图和机器人（无论是否有地图）
        node_num = (
            len(self.graph.node_coordinates)
            if hasattr(self.graph, "node_coordinates")
            else 0
        )
        if node_num > 0:
            # 绘制底图：细线边 + 空心圆圈节点
            node_radius = 4
            edge_width = 1
            for i in range(node_num):
                # 绘制节点：空心圆圈
                x, y = self.graph.node_coordinates[i]
                px, py = self.world_to_pixel(x, y)
                painter.setPen(QPen(QColor(80, 80, 80), 1))
                painter.setBrush(QBrush(Qt.NoBrush))  # 空心
                painter.drawEllipse(
                    px - node_radius, py - node_radius, node_radius * 2, node_radius * 2
                )
                # 绘制边（细线）
                for j in range(i + 1, node_num):
                    if (
                        hasattr(self.graph, "adj_matrix")
                        and i < len(self.graph.adj_matrix)
                        and j < len(self.graph.adj_matrix[i])
                    ):
                        if self.graph.adj_matrix[i][j] == 1:
                            jx, jy = self.graph.node_coordinates[j]
                            pjx, pjy = self.world_to_pixel(jx, jy)
                            painter.setPen(QPen(QColor(180, 180, 180), edge_width))
                            painter.drawLine(px, py, pjx, pjy)
            # 绘制信念状态（追捕者视角下的逃逸者可能位置）
            if (
                self.belief_pr is not None
                and self.belief_pos is not None
                and len(self.belief_pr) == node_num
            ):
                max_pr = np.max(self.belief_pr)
                belief_base_radius = 4
                belief_scale = 8  # 最大半径 = base + scale
                for i in range(node_num):
                    if self.belief_pos[i]:
                        x, y = self.graph.node_coordinates[i]
                        px, py = self.world_to_pixel(x, y)
                        pr = self.belief_pr[i]
                        gray_intensity = int(200 - pr * 200)
                        belief_color = QColor(gray_intensity, 255, gray_intensity)
                        radius = belief_base_radius + int(pr * belief_scale)
                        painter.setPen(QPen(belief_color.darker(), 1))
                        painter.setBrush(QBrush(belief_color))
                        painter.drawEllipse(
                            px - radius, py - radius, radius * 2, radius * 2
                        )
                        # 绘制置信度标签（可选，小字体）
                        if pr >= 0.1:  # 只显示显著置信
                            painter.setPen(QPen(Qt.black))
                            painter.setFont(QFont("Arial", 6))
                            painter.drawText(px + radius + 2, py + 2, f"{pr:.1f}")
            # 绘制真实机器人位置（监控视角）
            for robot in self.robots:
                if (
                    "position" not in robot
                    or robot["position"] is None
                    or "node" not in robot
                    or robot["node"] is None
                ):
                    continue
                try:
                    # 使用节点坐标绘制（更精确）
                    node_id = int(robot["node"])
                    if 0 <= node_id < node_num:
                        x, y = self.graph.node_coordinates[node_id]
                    else:
                        # 回退到解析字符串
                        pos_str = robot["position"].strip("()")
                        x, y = map(float, pos_str.split(","))
                    px, py = self.world_to_pixel(x, y)
                except:
                    continue
                # 根据角色选择颜色和样式
                role = robot.get("role", "unknown")
                color = self.role_colors.get(role, QColor(128, 128, 128))
                robot_radius = 10
                if role == "evader":
                    # 逃逸者：红色圆圈（空心）
                    painter.setPen(QPen(color, 2))
                    painter.setBrush(QBrush(Qt.NoBrush))
                else:  # pursuer
                    # 追捕者：蓝色实心圆
                    painter.setPen(QPen(color.darker(), 2))
                    painter.setBrush(QBrush(color.lighter(150)))
                painter.drawEllipse(
                    px - robot_radius,
                    py - robot_radius,
                    robot_radius * 2,
                    robot_radius * 2,
                )
                # 绘制机器人ID和角色
                painter.setPen(QPen(Qt.black))
                painter.setFont(QFont("Arial", 9, QFont.Bold))
                role_symbol = (
                    "E" if role == "evader" else "P" if role == "pursuer" else "?"
                )
                painter.drawText(px - 15, py - 15, f"{robot['id']}{role_symbol}")
                # 绘制目标位置（如果有）
                if (
                    "target" in robot
                    and robot["target"] != "未设置"
                    and "target_node" in robot
                    and robot["target_node"] != "未设置"
                ):
                    try:
                        target_node_id = int(robot["target_node"])
                        if 0 <= target_node_id < node_num:
                            tx, ty = self.graph.node_coordinates[target_node_id]
                        else:
                            target_str = robot["target"].strip("()")
                            tx, ty = map(float, target_str.split(","))
                        tpx, tpy = self.world_to_pixel(tx, ty)
                        # 绘制目标线和点
                        painter.setPen(QPen(color, 2, Qt.DashLine))
                        painter.drawLine(px, py, tpx, tpy)
                        painter.setPen(QPen(color.darker(), 2))
                        painter.setBrush(QBrush(Qt.NoBrush))
                        target_point_radius = 5
                        painter.drawEllipse(
                            tpx - target_point_radius,
                            tpy - target_point_radius,
                            target_point_radius * 2,
                            target_point_radius * 2,
                        )
                    except:
                        pass
                # 绘制状态指示器（外圈）
                status = robot.get("status", "unknown")
                if status == "active":
                    status_color = Qt.green
                elif status == "connecting":
                    status_color = Qt.yellow
                elif status == "disconnected":
                    status_color = Qt.red
                else:
                    status_color = Qt.gray
                painter.setPen(QPen(status_color, 2))
                painter.drawEllipse(
                    px - robot_radius - 3,
                    py - robot_radius - 3,
                    (robot_radius + 3) * 2,
                    (robot_radius + 3) * 2,
                )
            # 绘制标题和捕获次数
            painter.setPen(QPen(Qt.black))
            painter.setFont(QFont("Arial", 14, QFont.Bold))
            painter.drawText(20, 30, "追捕游戏 - 日志回放")
            painter.setFont(QFont("Arial", 12))
            painter.drawText(20, 60, f"捕获次数: {self.capture_count}")

    def resizeEvent(self, event):
        """窗口大小改变时重新计算缩放因子"""
        super().resizeEvent(event)
        if self.map_pixmap is not None:
            self.update_map_view()
        else:
            self.update_graph_view()

    def update_robots(self, robots, capture_count=0, belief_pr=None, belief_pos=None):
        """更新机器人状态和信念"""
        self.robots = robots
        self.belief_pr = belief_pr
        self.belief_pos = belief_pos
        self.capture_count = capture_count
        self.update()


class ReplayVideoWidget(QWidget):
    """回放视频显示面板"""

    def __init__(self, parent=None):
        super().__init__(parent)
        layout = QVBoxLayout(self)
        layout.addWidget(QLabel("回放视频"))
        self.video_label = QLabel()
        self.video_label.setMinimumSize(400, 300)
        self.video_label.setAlignment(Qt.AlignCenter)
        self.video_label.setStyleSheet(
            "border: 1px solid gray; background-color: black;"
        )
        layout.addWidget(self.video_label)

    def update_frame(self, frame):
        """更新视频帧"""
        if frame is not None:
            # cv2 BGR to RGB
            frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
            height, width, channel = frame_rgb.shape
            bytes_per_line = 3 * width
            q_image = QImage(
                frame_rgb.data, width, height, bytes_per_line, QImage.Format_RGB888
            )
            pixmap = QPixmap.fromImage(q_image)
            # 缩放到标签大小
            pixmap = pixmap.scaled(
                self.video_label.size(), Qt.KeepAspectRatio, Qt.SmoothTransformation
            )
            self.video_label.setPixmap(pixmap)
        else:
            self.video_label.clear()


class LogReplayUI(QMainWindow):
    def __init__(self, log_filename):
        super().__init__()
        self.setWindowTitle("不完全信息追捕游戏 - 日志回放")
        self.setGeometry(100, 100, 1200, 800)
        # 加载策略和图
        self.policy = PursuitPolicy()
        self.graph = self.policy
        # 加载解析器
        self.parser = LogbagParser(log_filename)
        self.timestamps = self.parser.get_timestamps()
        if not self.timestamps:
            raise ValueError("No entries in log file")
        self.min_t = self.timestamps[0]
        self.max_t = self.timestamps[-1]
        self.duration = (self.max_t - self.min_t).total_seconds()
        # 视频
        self.video_path = self._get_video_path(log_filename)
        self.cap = (
            cv2.VideoCapture(self.video_path)
            if self.video_path and os.path.exists(self.video_path)
            else None
        )
        # 回放状态
        self.elapsed = 0.0
        self.paused = True
        self.speed = 1.0
        self.last_real_time = time.time()
        # 创建主布局
        central_widget = QWidget()
        self.setCentralWidget(central_widget)
        main_layout = QVBoxLayout(central_widget)
        # 主分割器（水平）
        main_splitter = QSplitter(Qt.Horizontal)
        # 左侧：地图和时间滑块
        left_widget = QWidget()
        left_layout = QVBoxLayout(left_widget)
        # 地图组件
        self.map_widget = GraphMapWidget(self.graph)
        left_layout.addWidget(self.map_widget, 1)
        # 时间滑块
        slider_layout = QHBoxLayout()
        self.time_slider = QSlider(Qt.Horizontal)
        self.time_slider.setRange(0, int(self.duration * 1000))
        self.time_slider.valueChanged.connect(self.on_slider_moved)
        slider_layout.addWidget(QLabel("时间:"))
        slider_layout.addWidget(self.time_slider)
        slider_layout.addStretch()
        left_layout.addLayout(slider_layout)
        # 右侧：视频面板
        right_widget = QWidget()
        right_layout = QVBoxLayout(right_widget)
        self.video_widget = ReplayVideoWidget()
        right_layout.addWidget(self.video_widget)
        # 添加到主分割器
        main_splitter.addWidget(left_widget)
        main_splitter.addWidget(right_widget)
        main_splitter.setSizes([600, 600])
        main_layout.addWidget(main_splitter)
        # 控制和状态布局
        control_status_layout = QHBoxLayout()
        # 按钮
        self.play_btn = QPushButton("播放")
        self.play_btn.clicked.connect(self.toggle_play)
        control_status_layout.addWidget(self.play_btn)
        self.speed_up_btn = QPushButton("加速 x1.5")
        self.speed_up_btn.clicked.connect(lambda: self.change_speed(1.5))
        control_status_layout.addWidget(self.speed_up_btn)
        self.speed_down_btn = QPushButton("减速 /1.5")
        self.speed_down_btn.clicked.connect(lambda: self.change_speed(2 / 3))
        control_status_layout.addWidget(self.speed_down_btn)
        self.speed_label = QLabel("速度: x1.0")
        control_status_layout.addWidget(self.speed_label)
        control_status_layout.addStretch()
        # 状态标签
        self.time_label = QLabel(self.min_t.strftime("%H:%M:%S.%f")[:-3])
        control_status_layout.addWidget(self.time_label)
        self.belief_label = QLabel("信念: 支持位置 0 | 熵 0.00")
        control_status_layout.addWidget(self.belief_label)
        control_widget = QWidget()
        control_widget.setLayout(control_status_layout)
        control_widget.setFixedHeight(40)
        main_layout.addWidget(control_widget)
        # 底部状态栏
        self.status_bar = self.statusBar()
        self.status_bar.setFixedHeight(25)
        # 播放定时器
        self.play_timer = QTimer(self)
        self.play_timer.timeout.connect(self.advance_playback)
        self.play_timer.start(50)  # ~20 FPS
        # 初始更新
        self.update_display_from_elapsed()

    def _get_video_path(self, log_path):
        """从日志文件名构造视频路径"""
        base = os.path.splitext(os.path.basename(log_path))[0]
        match = re.match(r"logbag_(\d+)_(\d{8})_(\d{6})\.log$", base)
        if match:
            rand, ymd, hms = match.groups()
            video_base = f"log_video_{rand}_{ymd}_{hms}.mp4"
            return os.path.join(os.path.dirname(log_path), video_base)
        return None

    def toggle_play(self):
        """切换播放/暂停"""
        self.paused = not self.paused
        self.play_btn.setText("暂停" if not self.paused else "播放")

    def change_speed(self, multiplier):
        """改变播放速度"""
        self.speed *= multiplier
        self.speed = max(0.1, min(self.speed, 10.0))  # 限制范围
        self.speed_label.setText(f"速度: x{self.speed:.2f}")

    def on_slider_moved(self, value):
        """滑块移动时暂停并更新"""
        self.paused = True
        self.play_btn.setText("播放")
        self.elapsed = value / 1000.0
        self.update_display_from_elapsed()

    def advance_playback(self):
        """推进播放"""
        if not self.paused:
            delta = time.time() - self.last_real_time
            self.elapsed += delta * self.speed
            if self.elapsed > self.duration:
                self.elapsed = self.duration
                self.paused = True
                self.play_btn.setText("播放")
            self.time_slider.setValue(int(self.elapsed * 1000))
            self.last_real_time = time.time()
            self.update_display_from_elapsed()

    def update_display_from_elapsed(self):
        """从elapsed更新显示"""
        current_t = self.min_t + datetime.timedelta(seconds=self.elapsed)
        entry = self.parser.get_entry_by_time(current_t)
        if entry:
            positions = entry["positions"]
            beliefs = entry["beliefs"]
            # 构建beliefs数组
            node_num = self.policy.node_num
            belief_pos = np.zeros(node_num, dtype=bool)
            belief_pr = np.zeros(node_num)
            for node, pr in beliefs.items():
                if 0 <= node < node_num:
                    belief_pos[node] = True
                    belief_pr[node] = pr
            # 构建robots
            robots = []
            # Evader
            x, y = positions["evader"]
            if x != -1:
                node = self.policy.position_to_node(x, y)
                robots.append(
                    {
                        "id": 0,
                        "role": "evader",
                        "position": f"({x:.1f}, {y:.1f})",
                        "node": node,
                        "status": "active",
                    }
                )
            # p1
            x, y = positions["p1"]
            if x != -1:
                node = self.policy.position_to_node(x, y)
                robots.append(
                    {
                        "id": 1,
                        "role": "pursuer",
                        "position": f"({x:.1f}, {y:.1f})",
                        "node": node,
                        "status": "active",
                    }
                )
            # p2
            x, y = positions["p2"]
            if x != -1:
                node = self.policy.position_to_node(x, y)
                robots.append(
                    {
                        "id": 2,
                        "role": "pursuer",
                        "position": f"({x:.1f}, {y:.1f})",
                        "node": node,
                        "status": "active",
                    }
                )
            # 更新地图
            self.map_widget.update_robots(robots, 0, belief_pr, belief_pos)
            # 更新信念标签
            support = np.sum(belief_pos)
            entropy = (
                -np.sum(belief_pr[belief_pos] * np.log2(belief_pr[belief_pos] + 1e-10))
                if support > 0
                else 0.0
            )
            self.belief_label.setText(f"信念: 支持位置 {support} | 熵 {entropy:.2f}")
        # 更新视频
        if self.cap:
            self.cap.set(cv2.CAP_PROP_POS_MSEC, self.elapsed * 1000)
            ret, frame = self.cap.read()
            if ret:
                self.video_widget.update_frame(frame)
        # 更新时间标签
        time_str = current_t.strftime("%H:%M:%S.%f")[:-3]
        self.time_label.setText(time_str)

    def closeEvent(self, event):
        """关闭时释放资源"""
        if self.cap:
            self.cap.release()
        super().closeEvent(event)


if __name__ == "__main__":
    log_filename = "logbag_992711_20251019_18_44_26.log"
    app = QApplication(sys.argv)
    ui = LogReplayUI(log_filename)
    ui.show()
    sys.exit(app.exec_())
